#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include "left_arm.h"
#include <algorithm>

//! Initialize the action client and wait for action server to come up
LeftArm::LeftArm() {
  ROS_INFO("LeftArm()");
  // tell the action client that we want to spin a thread by default
  traj_client_ = new TrajClient("l_arm_controller/joint_trajectory_action", true);

  // wait for action server to come up
  while(!traj_client_->waitForServer(ros::Duration(5.0))) {
    ROS_INFO("Waiting for the joint_trajectory_action server");
  }
}

  //! Clean up the action client
  LeftArm::~LeftArm() {
  ROS_INFO("~LeftArm()");
    delete traj_client_;
  }

  bool LeftArm::waitForResult(float duration) {
    return traj_client_->waitForResult(ros::Duration(duration));
  }

  //! Sends the command to start a given trajectory
  void LeftArm::startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 0s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.0);
    traj_client_->sendGoal(goal);
  }

  pr2_controllers_msgs::JointTrajectoryGoal
      LeftArm::cameraToEyeTrajectory(float headPan, float duration) {
    ROS_INFO("Begin cameraToEyeTrajectory(%f)", headPan);
    // Bound the head pan goal between 0.5 and -0.5 radians,
    // or about 30 degrees. Then normalize it between -1 and 1.
    float headPanGoal = std::max(-0.5f, std::min(0.5f, headPan));
    headPanGoal *= 2.0f;
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("l_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(1);

    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    if (headPanGoal > 0.0f) {
      // Head is panned left.
      float alpha = headPanGoal;
      float beta = 1 - alpha;
      goal.trajectory.points[ind].positions[0] =
          (alpha * 1.5374603689239223) + (beta * 0.90156661379584158);
      goal.trajectory.points[ind].positions[1] =
          (alpha * -0.35415853084557625) + (beta * -0.35432772069178486);
      goal.trajectory.points[ind].positions[2] =
          (alpha * 0.13453835126896818) + (beta * 0.53141654653443648);
      goal.trajectory.points[ind].positions[3] =
          (alpha * -1.9229733505437328) + (beta * -1.7828353019244778);
      goal.trajectory.points[ind].positions[4] =
          (alpha * 0.69312168515300376) + (beta * 0.57170064964377731);
      goal.trajectory.points[ind].positions[5] =
          (alpha * -0.92749400884066979) + (beta * -0.7857419471561915);
      goal.trajectory.points[ind].positions[6] =
          (alpha * -4.99929493) + (beta * -4.55889764);
    } else {
      // Head is panned right.
      float alpha = -1 * headPanGoal;
      float beta = 1 - alpha;
      ROS_INFO("headPanGoal[%f], alpha[%f], beta[%f]",
          headPanGoal, alpha, beta);
      goal.trajectory.points[ind].positions[0] =
          (alpha * 0.075733798302906274) + (beta * 0.90156661379584158);
      goal.trajectory.points[ind].positions[1] =
          (alpha * -0.34383795022683278) + (beta * -0.35432772069178486);
      goal.trajectory.points[ind].positions[2] =
          (alpha * 0.51714496698751655) + (beta * 0.53141654653443648);
      goal.trajectory.points[ind].positions[3] =
          (alpha * -1.5015458097062835) + (beta * -1.7828353019244778);
      goal.trajectory.points[ind].positions[4] =
          (alpha * 0.82559151426978716) + (beta * 0.57170064964377731);
      goal.trajectory.points[ind].positions[5] =
          (alpha * -1.0531912790083977) + (beta * -0.7857419471561915);
      goal.trajectory.points[ind].positions[6] =
          (alpha * -4.59000652) + (beta * -4.55889764);
    }
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(duration);

    ROS_INFO("End cameraToEyeTrajectory()");
    //we are done; return the goal
    return goal;
  }

  pr2_controllers_msgs::JointTrajectoryGoal LeftArm::cameraFromEyeTrajectory() {
    ROS_INFO("Begin cameraFromEyeTrajectory()");
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("l_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("l_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("l_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("l_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("l_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("l_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("l_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(1);

    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = 0.75;//0.029057372730923237;
    //goal.trajectory.points[ind].positions[0] = 0.029057372730923237;
    goal.trajectory.points[ind].positions[1] = 0.59034378561566025;
    goal.trajectory.points[ind].positions[2] = -0.098777921099216348;
    goal.trajectory.points[ind].positions[3] = -2.1244941807896036;
    goal.trajectory.points[ind].positions[4] = 1.5051212446780216;
    goal.trajectory.points[ind].positions[5] = -1.7337578072062534;
    goal.trajectory.points[ind].positions[6] = 0.714004324;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);

    ROS_INFO("End cameraFromEyeTrajectory()");
    //we are done; return the goal
    return goal;
  }

  //! Returns the current state of the action
  actionlib::SimpleClientGoalState LeftArm::getState()
  {
    return traj_client_->getState();
  }

